#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTMotor)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     IR1,            sensorI2CCustomFastSkipStates)
#pragma config(Sensor, S3,     IR2,            sensorI2CCustomFastSkipStates)
#pragma config(Motor,  mtr_S1_C1_1,     L,             tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     R,             tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorCR,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     motorCL,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C4_1,     motorBall,     tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C4_2,     motorI,        tmotorNone, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    Bridge,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    Lift,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    Lift2,                tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of autonomous mode, you may want to perform some initialization on your robot.
// Things that might be performed during initialization include:
//   1. Move motors and servos to a preset position.
//   2. Some sensor types take a short while to reach stable values during which time it is best that
//      robot is not moving. For example, gyro sensor needs a few seconds to obtain the background
//      "bias" value.
//
// In many cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
  servo[Lift] = 8;
  servo[Bridge] = 240;
  servo[Lift2] = 248;
  return;
}



task main()
{
  initializeRobot();

  waitForStart(); // Wait for the beginning of autonomous phase.

  motor[L] = 85;
  motor[R] = 85;
  wait1Msec (5500);

  motor[R] = 0;
  motor[L] = 0;
  wait1Msec (5000);
  while (true)
  {}
}
